'''Date : O5th october 2012
	 Authors : Isaline Laurent (ens12ilt) - Gizem Berkdemir (ens12gbr)

	 This file contains all the functions using the laser, and used in order to detect and avoid obstacles 
'''

import json, time, httplib
from tools import *
from math import pi

HEADERS = {"Content-type": "application/json", "Accept": "text/json"}
URL = 'localhost:50000'
global params

def getLaser() :
		''' Request the server and returns a python object containing the loaded json laser'''
		co = httplib.HTTPConnection(URL)
		co.request('GET','/lokarria/laser/echoes')
		response = co.getresponse()
		if (response.status == 200):
				laser = response.read()
				response.close()
				return json.loads(laser)['Echoes']
		else:
				return 

def findAngle(laser, lookahead, index) :
		''' Find the best angle to rotate in order to avoid an obstacle laying on the index angle
					Input : laser, the loaded laser echo
									lookahead, the look ahead distance
									index, the index of the laser where the obstacle has been find 
					Output : the angle to rotate
		'''
		d = laser[0]
		c = int((pi/4) / params['AngleIncrement']) + 5
		s = 0
		for i in range(len(laser)) :
				if laser[i] > lookahead and abs(index - i) < abs(index - d) :
						m = 0
						for j in range(max(i-c, 0), min(i+c, params['LastIndex'])) :
								if (laser[j] <= lookahead) :
										m = 1
										break
								if m == 0 :	
										d = i
										s = 1
		if s!= 0 :						
				return (d - params['MiddleIndex']) * params['AngleIncrement']
		else :
				return pi


def warnZone(angle, r) :
		''' Determines wheiter the robot would find an obstacle on its way

				Input : angle, the rotation angle of the robot
								r, the radius of the arc it travel along
				Output : -1 if not in a warning zone, 1 if it does
								 the loaded laser echo
		'''
		laser = getLaser()
		c = int((pi/4) / params['AngleIncrement']) + 5
		il = int(abs(angle - params['StartAngle']) // params['AngleIncrement'])
		i1 = int(il - (angle // params['AngleIncrement']))
		if il > params['LastIndex'] :
				il = params['LastIndex']

		if angle >= 0 :
				a = 1
		else :
				a = -1
		
		if i1 < 0 :
				i1 = 0

		if il > params['LastIndex'] :
				il = params['LastIndex']

		for i in range(i1, il, a*1):
				ai = params['StartAngle'] + i*params['AngleIncrement']
				di = 2*r*sin((ai)/2)
				for j in range(max(i-c, 0),min(i+c, params['LastIndex'])):
						if laser[j] < di:
								return i, laser
		return -1, laser


def initLaser() :
		''' Initiate the laser parameters
					params['LastIndex']
					params['MiddleIndex']
					params['StartAngle']
					params['EndAngle']
					params['AngleIncrement']
		'''
		global params
		co = httplib.HTTPConnection(URL)
		co.request('GET','/lokarria/laser/properties')
		response = co.getresponse()
		if (response.status == 200):
				las_params = response.read()
				response.close()
				params = json.loads(las_params)
				params['LastIndex'] = int((params['EndAngle'] - params['StartAngle']) // params['AngleIncrement'])
				params['MiddleIndex'] = int(params['LastIndex'] // 3)
